/*
 www.pipoisu.com
 Copyright (c) 2011.  All rights reserved.

 File     : main.c
 Author(s): Erez Raviv

 This program is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.

 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 GNU General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with this program. If not, see <http://www.gnu.org/licenses/>.
*/

#include "main.h"


/*************************************************************************
 Global Variables
*************************************************************************/

float deltaT;

uint8_t operationsLed = 0;

int16_t	attitudeCMD[3];  // Pitch,Roll
uint8_t magnetometer = 0;


/*************************************************************************
 main
*************************************************************************/
int main(void)
{
    systemInit();

    LED_RED_ON; LED_GREEN_ON; // Green on = device initiating

    delay(STARTUP_DELAY); // allow rx to come online - green LED only ON
    // FlySky RX takes ~350ms to start up

    LED_GREEN_OFF; LED_RED_OFF;

    LED_GREEN_ON;LED_RED_OFF;

    // set up L3G4200D
    if(!l3g4200d_init())
        failureMode(0);

    // set up LSM3030DLHC
    if(!lsm303dlhc_init())
        failureMode(1);

//    if(!mpl3115a2_init())
//        failureMode(2);

    delay(250);

    calibrateGyro(); //flashes red led
//    calibrateCompassAccel();

    LED_RED_OFF; LED_GREEN_OFF; // leds off = ready to run!
    operationsLed = 1;

    uint32_t cycElapsed = 0;
    uint32_t cycLastCount = *DWT_CYCCNT;

    //main loop
    while (1)
    {
        //calculate deltaT
        // measure the time elapsed since the last time we checked
        uint32_t current_count = *DWT_CYCCNT;
        cycElapsed += current_count - cycLastCount;
        cycLastCount = current_count;

        // convert to microseconds
        deltaT = ((float)cycElapsed)*1000 / usTicks;

        // keep fractional microseconds for the next iteration
        cycElapsed %= usTicks;



        // calculate IMU and populate attitude vars
        if(magnetometer)
            updateAHRS();
        else
            updateIMU();

        getAttitude();



        //READ sensor data values
        populateSensorData();


        //run mixer
        mixer();
    }
}
